/*
 * SCOR.cpp
 *
 * Author: Samuel Queiroz
 */

#include "SCOR.h"

#include <stdio.h>

#include <pthread.h>

void SCOR::run()
{
    pthread_attr_t attr;

    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

    pthread_t threads[2];
	pthread_create(&threads[0], &attr, &Threadable::run_, gui);
	pthread_create(&threads[1], &attr, &Threadable::run_, sdlWrapper);

	/* Free attribute */
	pthread_attr_destroy(&attr);

	while (!keyboard->getStopButton()->isPressed())
	{
		treatAndMove();
	}

	gui->stop();
	sdlWrapper->stop();

	pthread_join(threads[0], NULL);
	pthread_join(threads[1], NULL);
}

void SCOR::treatAndMove()
{
	if (keyboard->getControlSwitcher()->isLeft())
	{
		treatKeyboardAndMove();
	}
	else
	{
		treatJoystickAndMove();
	}
}

void SCOR::treatKeyboardAndMove()
{
	int leftSpeed, rightSpeed;

	int x = keyboard->getRotationSlider()->getValue()
			* (keyboard->getRightButton()->isPressed() ? 1
					: keyboard->getLeftButton()->isPressed() ? -1
							: 0);

	int y = keyboard->getSpeedSlider()->getValue()
					* (keyboard->getUpButton()->isPressed() ? 1
							: keyboard->getDownButton()->isPressed() ? -1
									: 0);

	calculateSpeed(x, y, &leftSpeed, &rightSpeed);

	if (keyboard->getRobotSwitcher()->isLeft())
	{
		leftRobot.move(leftSpeed, rightSpeed);
	}
	else
	{
		rightRobot.move(leftSpeed, rightSpeed);
	}
}

void SCOR::treatJoystickAndMove()
{
	int leftSpeed, rightSpeed;

	calculateSpeed(joystick->getLeftButton()->getX(),
			joystick->getLeftButton()->getY(), &leftSpeed, &rightSpeed);
	leftRobot.move(leftSpeed, rightSpeed);

	calculateSpeed(joystick->getRightButton()->getX(),
			joystick->getRightButton()->getY(), &leftSpeed, &rightSpeed);
	rightRobot.move(leftSpeed, rightSpeed);
}

void SCOR::calculateSpeed(
		int x, int y,
		int * leftSpeed,
		int * rightSpeed)
{
	int yOffset;

	(*leftSpeed) = (*rightSpeed) = (yOffset = y > 0 ? POSITIVE_OFFSET : y < 0 ? NEGATIVE_OFFSET : 0) ? yOffset + y : (yOffset = 0);

	(*leftSpeed) += x > 0 ? (y ? (y > 0 ? x : -x) : MAXIMUM_SPEED) : 0;
	(*rightSpeed) += x < 0 ? (y ? (y > 0 ? -x : x) : MAXIMUM_SPEED) : 0;
}

int main()
{
	(new SCOR())->run();

	return 0;
}
